class Tank
	attr_reader :breakLoop
	# setting up tank default values
	def initialize (name="Tank01",port=2200)
		@@className = self.class.to_s.upcase+": "
		@@className = @@className.ljust(10)
		@speed=0
		@debug=true;
		@dir=0
		@deb = Debug.new(self)
		@sensorThreads =[]
		@port = port
		@consolePort = @port+1
		@motor1Speed = 0
		@motor2Speed = 0
		@sensorList = []
		
		@motorList = []
		@leftMotors = []
		@rightMotors = []
		@leftSpeed=0
		@rightSpeed=0
		@connected = false
		@consoleConnected = false
		#sensorThread = Thread.new {sendSensors}
		#sendSensors2
		#start logging
		@first0step=0;
		
		msg "Started logging"
		#@clientListenerThread = Thread.new {checkConnected}
		
		
		@descriptors = Array::new
		@serverSocket = TCPServer.new("",port)
		@serverSocket.setsockopt(Socket::SOL_SOCKET, Socket::SO_REUSEADDR, 1)
		
		@consoleDescriptors = Array::new
		@consoleServerSocket = TCPServer.new("",port+1)
		@consoleServerSocket.setsockopt(Socket::SOL_SOCKET, Socket::SO_REUSEADDR, 1)
	
		@descriptors.push(@serverSocket)
		@consoleDescriptors.push(@consoleServerSocket)
		@connected = false
		@consoleConnected = false
		@first0=true;
		
		@clientcount = 0
		@consoleClientcount = 0
		
		msg "Starting server"
		@runThread = Thread.new{run}
		msg "ClientServer listening to port #{port}"
		
		msg "Starting console server"
		@consoleRunThread = Thread.new{runConsole}
		msg "ConsoleServer listening to port #{port+1}"

	end
	

	
	def msg(message)
		
		@deb.msgg message
		#if @consoleSocket!=nil
		
		#	@consoleSocket.puts outmsg+"\r\n"
		if @consoleConnected
			@consoleDescriptors.each do |clisock|
				if clisock != @consoleServerSocket# && clisock != omit_sock
					clisock.puts(outmsg+"\r\n")
				end
			end
		
		
		#@logFile.puts outmsg
		#@logFile.flush
		end
	end

	def run # main server
		while 1
			res = select(@descriptors, nil, nil, nil)
			if res != nil then
				for sock in res[0]
					if sock == @serverSocket then
						accept_new_connection
					else
						if sock.eof? or @breakConnection then
							#str = sprintf("Client left %s:%s\n",sock.peeraddr[2],sock.peeraddr[1])
							#broadcast_string(str,sock)
							msg "Client (#{sock.peeraddr[2]}) disconnected"
							#msg str
							sock.close
							@descriptors.delete(sock)
							@clientcount = @clientcount-1
							if @clientcount==0
								@connected=false
								setDir(0)
								setSpeed(0)
							end
						else
							cmd = sock.gets.chop
							
							#cmd = sock.recv(20)
							#cmd = cmd.chop
							#str = sprintf("[%s|%s]: %s",sock.peeraddr[2],sock.peeraddr[1],cmd)
							#broadcast_string(str,sock)
							msg("Act on: "+cmd)
							messagefromCommand = parseCommand(cmd,sock)
							#msgC parseCommand(cmd,sock)
							if (messagefromCommand!="")
								msgC messagefromCommand
								msg messagefromCommand
							end
							#parseCommand(cmd,sock)
						end
					end
				end
			end
		end
	end #run
	
	
	
	
	def msgC(str) # send message to clients
		str<<"\r\n"
		@descriptors.each do |clisock|
			if clisock != @serverSocket# && clisock != omit_sock
				clisock.write(str)
			end
		end
	end #message to client
	
	def accept_new_connection
		newsock = @serverSocket.accept
		@lastSock = newsock
		@connected=true
		@clientcount = @clientcount+1
		@descriptors.push(newsock)
		msgC "Welcome"
		msgC "Sepearate commands with <ENTER>"
		msgC "type QUIT to disconnect"	  
		msgC "and type SHUTDOWN to shutdown the server"	  
		showCapabilities
		str = "Client joined from (#{newsock.peeraddr[2]})"
		msg(str)
	end #accept_new_connection

	def accept_new_console_connection
		newsock = @consoleServerSocket.accept
		@consoleConnected=true
		@consoleClientcount = @consoleClientcount+1
		@consoleDescriptors.push(newsock)
		newsock.write("Welcome to the console\r\n")
	end #accept_new_connection

	
	# How to act on different commands
	def showCapabilities
		msgC "Capabilities: "
		@sensorList.each {
			|sensor|
			msgC "Sensor: "+sensor.name+"=("+sensor.minEU.to_s+" - "+sensor.maxEU.to_s+") 0"
		}
		@motorList.each{
			|motor|
			msgC "Motor: "+motor.name+".sp=(-100 - 100) 0"
		}
	end
	
	def parseCommand(cmd,sock)
		msg "Command \"#{cmd}\" from (#{sock.peeraddr[2]})"
		fb =""
		case cmd.upcase
			when "SHUTDOWN"
				msgC "Shutting down Server"
				msg "Shutting down Server"
				@breakConnection=true
				@breakLoop=true
			when "QUIT"
				#@client.puts "Disconnected\r\n"
				msgC "Disconnected\r\n"
				msg "Client (#{sock.peeraddr[2]}) disconnected"
				sock.close
				@descriptors.delete(sock)
				@clientcount = @clientcount-1
				if @clientcount==0
					@connected=false
				end
         when "EXIT"
				#@client.puts "Disconnected\r\n"
				msgC "Disconnected\r\n"
				msg "Client (#{sock.peeraddr[2]}) disconnected"
				sock.close
				@descriptors.delete(sock)
				@clientcount = @clientcount-1
				if @clientcount==0
					@connected=false
				end
			when /CAPABILITIES/
				showCapabilities
			when /SPEED=(.*)/
				setSpeed($1)
			when /DIR=(.*)/
				setDir($1)
         when /RAMP=(.*)/
				fb = setRamp($1)
			when /RESTART/
				exit
				
			when "HELP"
				fb = "quit shutdown speed=xxx dir=xxx restart ramp=on|off"
			else
				fb = "Unknown Command: \"#{cmd}\""
		end
		return fb
	end
	
	# methods
        def setRamp(useRamping)
          msg "Ramp \"#{useRamping}\""
          if(useRamping.upcase=="ON")
            @motorList.each{|motor|motor.useRamping=true}
				return "motors are now using ramping"
          else
            @motorList.each{|motor|motor.useRamping=false}
				return "motors are now not using ramping"
          end
        end
        
	def setSpeed(speed)
		@speed = speed.to_i
		regulator
	end
	def setDir(dir)
		@dir = dir.to_i
		regulator
	end
	
	def regulator
		if @speed>100 then @speed=100 end
		if @speed<-100 then @speed=-100 end
		if @dir>100 then @dir=100 end
		if @dir<-100 then @dir=-100 end
		

		
#		if(@speed.to_i !=0)
#			@first0step=0
#			sub = (@dir.to_i.abs)*@speed.to_i/100
#			if(@dir.to_i<0)
#				@leftSpeed=@speed.to_i-sub
#				@rightSpeed=@speed.to_i
#			else
#				@leftSpeed=@speed.to_i
#				@rightSpeed=@speed.to_i-sub
#			end
#		else
		if(@speed.to_i !=0) then
			@first0step=0
			sub = (@dir.to_i.abs)*@speed.to_i/100
			if sub>99 then sub=99 end
			if(@dir.to_i<0)
					@leftSpeed=@speed.to_i
					@rightSpeed=@speed.to_i-sub
			else
					@leftSpeed=@speed.to_i-sub
					@rightSpeed=@speed.to_i
			end
		else



                  msg "got 0 speed"
		if(@first0step==0)then
			@first0step=1
			msg "firststep"
			@motor1Speed=0
			@motor2Speed=0
         @leftSpeed=0
         @rightSpeed=0
		end
		
		if(@first0step==1) then
			if(@dir.to_i==0)	#dont allow rotate around himself if dir<>0
				@first0step=2
            msg "progress to firststep 2"
			end
         msg "stuck at firststep 1"
			#@ii = @ii+1
		end
		if(@first0step==2) then
			# rotate in place
         msg "at firststep 2, rotate in place"
			#Left Motors
			#@leftMotors.each {|motor| motor.speedSp = @speed.to_i}
			@leftSpeed=(@dir*(-0.5)).to_i
			#@motor1Speed=@dir.to_i
			
			#Right Motors
			#@rightMotors.each {|motor| motor.speedSp = @speed.to_i*(-1)}
			@rightSpeed=(@dir*( 0.5)).to_i
			#@motor2Speed=@dir.to_i*(-1)
			#@first0step=0
			msg "step3"
			end
		end
		#msg "Motor1speed="+@motor1Speed.to_s
		#msg "LeftMotors: "+@leftMotors.length
		@leftMotors.each {|lmotor| lmotor.speedSp = @leftSpeed}   #send speed to motors
		@rightMotors.each {|rmotor|rmotor.speedSp = @rightSpeed}  #send speed to motors
		
		#msg "Motor1speed="+@leftSpeed.to_s
		#msg "Motor2speed="+@rightSpeed.to_s
		#msgC "M1.sp="+@leftSpeed.to_s
		#msgC "M2.sp="+@rightSpeed.to_s
		@motorList.each{
			|motor|
			msgC motor.name+".sp="+motor.speedSp.to_s
			msg motor.name+".sp="+motor.speedSp.to_s
		}
		# use speed and dir to set speed on two motors
	end
	
	def addSensors(theSensor)
		msg "Attached sensor: "+theSensor.name
		@sensorList << theSensor
		@sensorThreads << Thread.new {sensorWriter(theSensor)}
	end

	def addMotor(theMotor,side="left")
		msg "Attached motor: "+theMotor.name+" to the "+side+" side"
		@motorList << theMotor
		
		if(side=="left")
			msg "added to left side"
			@leftMotors << theMotor
		else
			msg "added to right side"
			@rightMotors << theMotor
		end
	end
	
	
	
	
	def sensorWriter(aSensor) #sends data to client
		while true
			if @connected	# if client, then
				msgC aSensor.name+"="+aSensor.value.to_s
			end
			sleep aSensor.writeInterval
		end
	end
	
	def runConsole #consoleServer
		while 1
			res = select(@consoleDescriptors, nil, nil, nil)
			if res != nil then
				for sock in res[0]
					if sock == @consoleServerSocket then
						accept_new_console_connection
					else
						if sock.eof? then
							sock.close
							@consoleDescriptors.delete(sock)
							@consoleClientcount = @consoleClientcount-1
							if @consoleClientcount==0
								@consoleConnected=false
							end
						else
							cmd = sock.gets()
							cmd = cmd.chop
							#broadcast_string(str,sock)
							#msg("Act on: "+cmd)
							#msg cmd
						end
					end
				end
			end
		end
	end #run

	
	
	
end